#include <iostream>
#include <Eigen/Dense>
#include "rotation.h"
using namespace std;
using namespace Eigen;
#ifndef IMU_ALGORITHM_V2_0_CLASS_H
#define IMU_ALGORITHM_V2_0_CLASS_H
class IMUDATA{
public:
    IMUDATA(Vector3d newGyroData,
            Vector3d newAccelData,
            double newTime);
    IMUDATA();
    ~IMUDATA();
    IMUDATA(const IMUDATA &imuData1);
    Vector3d getGyroData() const {return gyroData;}
    Vector3d getAccelData() const {return accelData;}
    double getGyroFront() const {return gyroData[0];}
    double getGyroRight() const {return gyroData[1];}
    double getGyroDown() const {return gyroData[2];}
    double getAccelFront() const {return accelData[0];}
    double getAccelRight() const {return accelData[1];}
    double getAccelDown() const {return accelData[2];}
    double getTime() const {return time;}
    void showIMUDATA() const;
    void changeGyroData(Vector3d gyroData1){gyroData = gyroData1;}
    void changeAccelData(Vector3d accelData1){accelData = accelData1;}
    void changeTime(double time1){time = time1;}
private:
    Vector3d gyroData;
    Vector3d accelData;
    double time;
};



///////////////////////////////////class State
class State{//radian
public:
    State(Vector3d new_att,
          Vector3d new_vel,
          Vector3d new_pos,
          double new_time);
    State(Quaterniond new_att_quat,
          Vector3d new_vel,
          Vector3d new_pos,
          double new_time);
    State(Matrix3d new_att_mat,
          Vector3d new_vel,
          Vector3d new_pos,
          double new_time);
    State();
    State(const State &state1);
    ~State();
    Quaterniond get_att_quat() const {return att_quat;}
    Matrix3d get_att_mat() const {return Rotation::quaternion2matrix(att_quat);}
    Vector3d get_att() const {return Rotation::quaternion2euler(att_quat);}
    Vector3d get_vel() const {return vel;}
    Vector3d get_pos() const {return pos;}
    double getRoll() const {return get_att()[0];}
    double getPitch() const {return get_att()[1];}
    double getHeading() const {return get_att()[2];}
    double getVNorth() const {return vel[0];}
    double getVEast() const {return vel[1];}
    double getVDown() const {return vel[2];}
    double getLatitude() const {return pos[0];}
    double getLongitude() const {return pos[1];}
    double getHeight() const {return pos[2];}
    double getTime() const {return time;}
    void showStateInAngle() const;
    void showStateInRadian() const;
    void changeState(Vector3d new_att,
                     Vector3d new_vel,
                     Vector3d new_pos,
                     double new_time);
    void changeState(Quaterniond new_att_quat,
                     Vector3d new_vel,
                     Vector3d new_pos,
                     double new_time);
private:
    Quaterniond att_quat;
    Vector3d vel;
    Vector3d pos;
    double time;
};




//////////////////////////////////////class GNSSDATA
class GNSSDATA{
public:
    GNSSDATA(Vector3d position, Vector3d positionStd, double time);
    GNSSDATA();
    GNSSDATA(const GNSSDATA &gnssData1);
    ~GNSSDATA();
    double getTime() const {return time;}
    double getLatitude() const {return position[0];}
    double getLongitude() const {return position[1];}
    double getHeight() const {return position[2];}
    double getLatitudeStd() const {return positionStd[0];}
    double getLongitudeStd() const {return positionStd[1];}
    double getHeightStd() const {return positionStd[2];}
    Vector3d getposition() const {return position;}
    Vector3d getPositionStd() const {return positionStd;}
    void showGnssData() const;
private:
    Vector3d position;
    Vector3d positionStd;
    double time;
};







#endif //IMU_ALGORITHM_V2_0_CLASS_H
